#include "matrixLib.h"
#include <stdio.h>
#include <string.h>
#include <math.h>

//Function to add matrices --> C = A+B
void addMatrix(kalmanType *A, kalmanType *B, kalmanType *C, int len, bool isSubtract)
{
	for(int count=0; count < len; count++)
	{
		if(isSubtract == true)
		{
			C[count] = A[count] - B[count];
		}
		else
		{
			C[count] = A[count] + B[count];
		}
		
	}
}

//Function to multiply matrices --> C = A*B
void multMatrix(kalmanType *A, kalmanType *B, kalmanType *C, int A_rows, int A_cols, int B_cols)
{
	kalmanType sum;
	int C_rows = A_rows;
	int C_cols = B_cols;

	for (int i=0; i < A_rows; i++)
	{
		for (int k=0; k < B_cols; k++)
		{
			sum = 0;
			for (int j=0; j < A_cols; j++)
				sum = sum +  A[i*A_cols + j] * B[j*B_cols + k];

			C[i*C_cols + k] = sum; 
		}
	}
}

void matTranspose(kalmanType *curMat, int numRows, int numCols)
{
    kalmanType *tmp_transposeCopy = new kalmanType[numRows*numCols];
    
	for(int row=0; row < numRows; row++)
	{
		for(int col=0; col < numCols; col++)
		{
			tmp_transposeCopy[col*numRows + row] = curMat[row*numCols + col];
		}
	}

	memcpy(curMat, tmp_transposeCopy, numRows*numCols*sizeof(kalmanType));
	delete tmp_transposeCopy;
}


void normalizeVector(kalmanType *vec, int len)
{
	double norm = 0;

	for(int count=0; count < len; count++)
	{
		norm += vec[count]*vec[count];
	}

	for(int count=0; count < len; count++)
	{
		vec[count] /= sqrt(norm);
	}
}


void dbg_PrintMat(kalmanType* argMat, int numRows, int numCols)
{
	for(int rowCount=0; rowCount < numRows; rowCount++)
	{
		for(int colCount=0; colCount < numCols; colCount++)
		{
			printf("%lf ", argMat[rowCount*numCols + colCount]);
		}
		printf("\n");
	}
	printf("\n");
}

void q2qmat_c(kalmanType *quaternion, kalmanType *qmat)
{
	qmat[0]=-(quaternion[1]); qmat[1]=-(quaternion[2]); qmat[2]=-(quaternion[3]);
	qmat[3] =quaternion[0]; qmat[4] =-(quaternion[3]); qmat[5] =quaternion[2];
	qmat[6] =quaternion[3]; qmat[7] =quaternion[0]; qmat[8] =-(quaternion[1]);
	qmat[9] =-(quaternion[2]); qmat[10] =quaternion[1]; qmat[11] =quaternion[0];
}

void q_turn(kalmanType* quaternion, kalmanType* pqrdt2, kalmanType* quaternionMat, kalmanType* tmp_multResult_4, kalmanType* updatedQuaternion)
{
	q2qmat_c(quaternion,quaternionMat);
	multMatrix(quaternionMat, pqrdt2, tmp_multResult_4, 4, 3, 1);
	addMatrix(quaternion, tmp_multResult_4, updatedQuaternion, 4, 0);
	normalizeVector(updatedQuaternion, 4);
}
